#include <devices_ext/radar.h>
#include <devices/uart.h>
#include <devices_ext/uartutils.h>

/* 因为超声波必须有足够的延时来读入数据，所以这个值不应该经常被改动，虽然留有接口 */
static unsigned int timeout = 150;

void radar_write( RadarPort p, uint8_t b1, uint8_t b2, uint8_t b3 )
{
	uint8_t buf[4] = { b1, b2, b3 };
	buf[3] = b1 + b2 + b3;
	uart_write( p, buf, 4 );
}

int radar_recv( RadarPort p, void *buf, uint8_t header )
{
	int rd = uart_read_header_msg( p, buf, 4, header, timeout );
	if( rd < 4 )
		return rd;
	uint8_t *recv = buf;
	return radar_validate( recv ) ? 0 : 4;
}

/* 从一串超声波返回消息中提取距离，比起直接用指针来保存返回值，现在的写法更明确些 */
int radar_dist( RadarMsg msg )
{
	uint16_t ret;
	uint8_t *dest = ( uint8_t * )&ret;
	dest[1] = msg[1]; //高位装低位
	dest[0] = msg[2]; //低位装高位
	if( ret > 500 ){
		return 501;
	}
	else{
		return ret;
	}
}

/* Temp = Temperature */
int radar_temperature( RadarMsg msg )
{
	return radar_dist( msg );
}

void radar_set_timeout( unsigned int to )
{
	timeout = to;
}

#ifdef _STDIO_H_
void printRadarMsg( RadarMsg msg )
{
	uint8_t *buf = msg;
	printf( "R: %02X %02X %02X %02X\n", buf[0], buf[1], buf[2], buf[3] );
}
#endif

